//
// math.c - Math library
//


#include "../../qcommon/qcommon.h"




vec3_t	axisDefault[3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
mat4_t	matrixIdentity = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};

int Q_PrintStrlen (const char *string){

	int		len = 0;

	while (*string){
		if (Q_IsColorString(string)){
			string += 2;
			continue;
		}

		string++;
		len++;
	}

	return len;
}

// TODO: Replace this
void Com_StripExtension (const char *path, char *dst, int dstSize){

	const char	*s, *last;

	s = last = path + strlen(path);
	while (*s != '/' && *s != '\\' && s != path){
		if (*s == '.'){
			last = s;
			break;
		}

		s--;
	}

	Q_strncpyz(dst, path, dstSize);
	if (last-path < dstSize)
		dst[last-path] = 0;
}

void Com_FileExtension (const char *path, char *dst, int dstSize){

	const char	*s, *last;

	s = last = path + strlen(path);
	while (*s != '/' && *s != '\\' && s != path){
		if (*s == '.'){
			last = s;
			break;
		}

		s--;
	}

	Q_strncpyz(dst, last, dstSize);

}
float AngleMod (float angle){

	return (360.0/65536) * ((int)(angle * (65536/360.0)) & 65535);
}

void VectorToAngles (const vec3_t vec, vec3_t angles){

	float	forward;
	float	pitch, yaw;

	if (vec[0] == 0.0 && vec[1] == 0.0){
		yaw = 0.0;

		if (vec[2] > 0.0)
			pitch = 90;
		else
			pitch = 270;
	}
	else {
		if (vec[0])
			yaw = RAD2DEG(atan2(vec[1], vec[0]));
		else if (vec[1] > 0.0)
			yaw = 90;
		else
			yaw = 270;

		if (yaw < 0.0)
			yaw += 360;

		forward = sqrt(vec[0]*vec[0] + vec[1]*vec[1]);

		pitch = RAD2DEG(atan2(vec[2], forward));
		if (pitch < 0.0)
			pitch += 360;
	}

	angles[PITCH] = -pitch;
	angles[YAW] = yaw;
	angles[ROLL] = 0.0;
}

void VectorReflect (const vec3_t v, const vec3_t normal, vec3_t out){

	float	d;

	d = 2.0 * (v[0]*normal[0] + v[1]*normal[1] + v[2]*normal[2]);

	out[0] = v[0] - normal[0] * d;
	out[1] = v[1] - normal[1] * d;
	out[2] = v[2] - normal[2] * d;
}

void LerpAngles (const vec3_t from, const vec3_t to, float frac, vec3_t out){

	int		i;

	for (i = 0; i < 3; i++){
		if (to[i] - from[i] > 180.0){
			out[i] = from[i] + ((to[i] - 360.0) - from[i]) * frac;
			continue;
		}
		if (to[i] - from[i] < -180.0){
			out[i] = from[i] + ((to[i] + 360.0) - from[i]) * frac;
			continue;
		}

		out[i] = from[i] + (to[i] - from[i]) * frac;
	}
}

int PointOnPlaneSide2 (const vec3_t point, float epsilon, const cplane_t *plane){

	float	dist;

	if (plane->type < PLANE_NON_AXIAL)
		dist = point[plane->type] - plane->dist;
	else
		dist = DotProduct(point, plane->normal) - plane->dist;

	if (dist > epsilon)
		return SIDE_FRONT;
	if (dist < -epsilon)
		return SIDE_BACK;

	return SIDE_ON;
}

int NearestPowerOfTwo (int number, int roundDown){

	int		n = 1;

	if (number <= 0)
		return 1;

	while (n < number)
		n <<= 1;

	if (roundDown){
		if (n > number)
			n >>= 1;
	}

	return n;
}

bool IsPowerOfTwo (int number){

	if (number <= 0)
		return false;

	if ((number & (number - 1)) != 0)
		return false;

	return true;
}

void VectorMin (const vec3_t v1, const vec3_t v2, vec3_t out){

	out[0] = (v1[0] < v2[0]) ? v1[0] : v2[0];
	out[1] = (v1[1] < v2[1]) ? v1[1] : v2[1];
	out[2] = (v1[2] < v2[2]) ? v1[2] : v2[2];
}

void VectorMax (const vec3_t v1, const vec3_t v2, vec3_t out){

	out[0] = (v1[0] > v2[0]) ? v1[0] : v2[0];
	out[1] = (v1[1] > v2[1]) ? v1[1] : v2[1];
	out[2] = (v1[2] > v2[2]) ? v1[2] : v2[2];
}

void SnapVector (vec3_t v){

	v[0] = (int)v[0];
	v[1] = (int)v[1];
	v[2] = (int)v[2];
}

float Q_rsqrt (float number){

	int		i;
	float	x, y;

	x = number * 0.5f;
	i = *(int *)&number;			// Evil floating point bit level hacking
	i = 0x5F3759DF - (i >> 1);		// What the fuck?
	y = *(float *)&i;
	y = y * (1.5f - (x * y * y));	// First iteration

	return y;
}

bool BoundsAndSphereIntersect (const vec3_t mins, const vec3_t maxs, const vec3_t center, float radius){

	if (mins[0] > center[0] + radius || mins[1] > center[1] + radius || mins[2] > center[2] + radius)
		return false;
	if (maxs[0] < center[0] - radius || maxs[1] < center[1] - radius || maxs[2] < center[2] - radius)
		return false;

	return true;
}

int BoxOnPlaneSide2 (const vec3_t mins, const vec3_t maxs, const cplane_t *plane){

	float	dist1, dist2;

	// Fast axial cases
	if (plane->type < PLANE_NON_AXIAL){
		if (plane->dist <= mins[plane->type])
			return SIDE_FRONT;
		if (plane->dist >= maxs[plane->type])
			return SIDE_BACK;

		return SIDE_CROSS;
	}
	
	// General case
	switch (plane->signbits){
	case 0:
		dist1 = plane->normal[0]*maxs[0] + plane->normal[1]*maxs[1] + plane->normal[2]*maxs[2];
		dist2 = plane->normal[0]*mins[0] + plane->normal[1]*mins[1] + plane->normal[2]*mins[2];
		break;
	case 1:
		dist1 = plane->normal[0]*mins[0] + plane->normal[1]*maxs[1] + plane->normal[2]*maxs[2];
		dist2 = plane->normal[0]*maxs[0] + plane->normal[1]*mins[1] + plane->normal[2]*mins[2];
		break;
	case 2:
		dist1 = plane->normal[0]*maxs[0] + plane->normal[1]*mins[1] + plane->normal[2]*maxs[2];
		dist2 = plane->normal[0]*mins[0] + plane->normal[1]*maxs[1] + plane->normal[2]*mins[2];
		break;
	case 3:
		dist1 = plane->normal[0]*mins[0] + plane->normal[1]*mins[1] + plane->normal[2]*maxs[2];
		dist2 = plane->normal[0]*maxs[0] + plane->normal[1]*maxs[1] + plane->normal[2]*mins[2];
		break;
	case 4:
		dist1 = plane->normal[0]*maxs[0] + plane->normal[1]*maxs[1] + plane->normal[2]*mins[2];
		dist2 = plane->normal[0]*mins[0] + plane->normal[1]*mins[1] + plane->normal[2]*maxs[2];
		break;
	case 5:
		dist1 = plane->normal[0]*mins[0] + plane->normal[1]*maxs[1] + plane->normal[2]*mins[2];
		dist2 = plane->normal[0]*maxs[0] + plane->normal[1]*mins[1] + plane->normal[2]*maxs[2];
		break;
	case 6:
		dist1 = plane->normal[0]*maxs[0] + plane->normal[1]*mins[1] + plane->normal[2]*mins[2];
		dist2 = plane->normal[0]*mins[0] + plane->normal[1]*maxs[1] + plane->normal[2]*maxs[2];
		break;
	case 7:
		dist1 = plane->normal[0]*mins[0] + plane->normal[1]*mins[1] + plane->normal[2]*mins[2];
		dist2 = plane->normal[0]*maxs[0] + plane->normal[1]*maxs[1] + plane->normal[2]*maxs[2];
		break;
	default:
		dist1 = 0.0;
		dist2 = 0.0;
		break;
	}

	if (dist2 < plane->dist){
		if (dist1 >= plane->dist)
			return SIDE_CROSS;
		else
			return SIDE_BACK;
	}

	return SIDE_FRONT;
}

bool PlaneFromPoints (cplane_t *plane, const vec3_t a, const vec3_t b, const vec3_t c){
	
	vec3_t	edge[2];

	VectorSubtract(b, a, edge[0]);
	VectorSubtract(c, a, edge[1]);
	CrossProduct(edge[1], edge[0], plane->normal);

	if (!VectorNormalize(plane->normal)){
		plane->dist = 0.0;
		return false;
	}

	plane->dist = DotProduct(a, plane->normal);
	return true;
}




int SphereOnPlaneSide2 (const vec3_t center, float radius, const cplane_t *plane){

	float	dist;

	if (plane->type < PLANE_NON_AXIAL)
		dist = center[plane->type] - plane->dist;
	else
		dist = DotProduct(center, plane->normal) - plane->dist;

	if (dist > radius)
		return SIDE_FRONT;
	if (dist < -radius)
		return SIDE_BACK;

	return SIDE_CROSS;
}

void SetPlaneSignbits (cplane_t *plane){

	plane->signbits = 0;

	if (plane->normal[0] < 0.0)
		plane->signbits |= 1;
	if (plane->normal[1] < 0.0)
		plane->signbits |= 2;
	if (plane->normal[2] < 0.0)
		plane->signbits |= 4;
}


void Matrix4_AffineInverse (const mat4_t in, mat4_t out){

	out[ 0] = in[ 0];
	out[ 1] = in[ 4];
	out[ 2] = in[ 8];
	out[ 3] = 0.0;
	out[ 4] = in[ 1];
	out[ 5] = in[ 5];
	out[ 6] = in[ 9];
	out[ 7] = 0.0;
	out[ 8] = in[ 2];
	out[ 9] = in[ 6];
	out[10] = in[10];
	out[11] = 0.0;
	out[12] = -(in[ 0] * in[12] + in[ 1] * in[13] + in[ 2] * in[14]);
	out[13] = -(in[ 4] * in[12] + in[ 5] * in[13] + in[ 6] * in[14]);
	out[14] = -(in[ 8] * in[12] + in[ 9] * in[13] + in[10] * in[14]);
	out[15] = 1.0;
}

void VectorRotate (const vec3_t v, const vec3_t matrix[3], vec3_t out){

	out[0] = v[0]*matrix[0][0] + v[1]*matrix[0][1] + v[2]*matrix[0][2];
	out[1] = v[0]*matrix[1][0] + v[1]*matrix[1][1] + v[2]*matrix[1][2];
	out[2] = v[0]*matrix[2][0] + v[1]*matrix[2][1] + v[2]*matrix[2][2];
}

bool AxisCompare (const vec3_t axis1[3], const vec3_t axis2[3]){

	if (axis1[0][0] != axis2[0][0] || axis1[0][1] != axis2[0][1] || axis1[0][2] != axis2[0][2])
		return false;
	if (axis1[1][0] != axis2[1][0] || axis1[1][1] != axis2[1][1] || axis1[1][2] != axis2[1][2])
		return false;
	if (axis1[2][0] != axis2[2][0] || axis1[2][1] != axis2[2][1] || axis1[2][2] != axis2[2][2])
		return false;

	return true;
}

void Matrix4_Copy (const mat4_t in, mat4_t out){

	out[ 0] = in[ 0];
	out[ 1] = in[ 1];
	out[ 2] = in[ 2];
	out[ 3] = in[ 3];
	out[ 4] = in[ 4];
	out[ 5] = in[ 5];
	out[ 6] = in[ 6];
	out[ 7] = in[ 7];
	out[ 8] = in[ 8];
	out[ 9] = in[ 9];
	out[10] = in[10];
	out[11] = in[11];
	out[12] = in[12];
	out[13] = in[13];
	out[14] = in[14];
	out[15] = in[15];
}

bool Matrix4_Compare (const mat4_t m1, const mat4_t m2){

	if (m1[ 0] != m2[ 0])
		return false;
	if (m1[ 1] != m2[ 1])
		return false;
	if (m1[ 2] != m2[ 2])
		return false;
	if (m1[ 3] != m2[ 3])
		return false;
	if (m1[ 4] != m2[ 4])
		return false;
	if (m1[ 5] != m2[ 5])
		return false;
	if (m1[ 6] != m2[ 6])
		return false;
	if (m1[ 7] != m2[ 7])
		return false;
	if (m1[ 8] != m2[ 8])
		return false;
	if (m1[ 9] != m2[ 9])
		return false;
	if (m1[10] != m2[10])
		return false;
	if (m1[11] != m2[11])
		return false;
	if (m1[12] != m2[12])
		return false;
	if (m1[13] != m2[13])
		return false;
	if (m1[14] != m2[14])
		return false;
	if (m1[15] != m2[15])
		return false;

	return true;
}

void Matrix4_Identity (mat4_t m){

	m[ 0] = 1.0;
	m[ 1] = 0.0;
	m[ 2] = 0.0;
	m[ 3] = 0.0;
	m[ 4] = 0.0;
	m[ 5] = 1.0;
	m[ 6] = 0.0;
	m[ 7] = 0.0;
	m[ 8] = 0.0;
	m[ 9] = 0.0;
	m[10] = 1.0;
	m[11] = 0.0;
	m[12] = 0.0;
	m[13] = 0.0;
	m[14] = 0.0;
	m[15] = 1.0;
}

void Matrix4_MultiplyFast (const mat4_t m1, const mat4_t m2, mat4_t out){

	out[ 0] = m1[ 0] * m2[ 0] + m1[ 4] * m2[ 1] + m1[ 8] * m2[ 2];
	out[ 1] = m1[ 1] * m2[ 0] + m1[ 5] * m2[ 1] + m1[ 9] * m2[ 2];
	out[ 2] = m1[ 2] * m2[ 0] + m1[ 6] * m2[ 1] + m1[10] * m2[ 2];
	out[ 3] = 0.0;
	out[ 4] = m1[ 0] * m2[ 4] + m1[ 4] * m2[ 5] + m1[ 8] * m2[ 6];
	out[ 5] = m1[ 1] * m2[ 4] + m1[ 5] * m2[ 5] + m1[ 9] * m2[ 6];
	out[ 6] = m1[ 2] * m2[ 4] + m1[ 6] * m2[ 5] + m1[10] * m2[ 6];
	out[ 7] = 0.0;
	out[ 8] = m1[ 0] * m2[ 8] + m1[ 4] * m2[ 9] + m1[ 8] * m2[10];
	out[ 9] = m1[ 1] * m2[ 8] + m1[ 5] * m2[ 9] + m1[ 9] * m2[10];
	out[10] = m1[ 2] * m2[ 8] + m1[ 6] * m2[ 9] + m1[10] * m2[10];
	out[11] = 0.0;
	out[12] = m1[ 0] * m2[12] + m1[ 4] * m2[13] + m1[ 8] * m2[14] + m1[12];
	out[13] = m1[ 1] * m2[12] + m1[ 5] * m2[13] + m1[ 9] * m2[14] + m1[13];
	out[14] = m1[ 2] * m2[12] + m1[ 6] * m2[13] + m1[10] * m2[14] + m1[14];
	out[15] = 1.0;
}

void Matrix4_Rotate (mat4_t m, float angle, float x, float y, float z){

	vec4_t	mx, my, mz;
	vec3_t	rx, ry, rz;
	float	mag, rad, s, c, i;
	float	xx, yy, zz, xy, yz, zx, xs, ys, zs;

	mag = sqrt(x*x + y*y + z*z);
	if (mag == 0.0)
		return;

	mag = 1.0 / mag;

	x *= mag;
	y *= mag;
	z *= mag;

	rad = DEG2RAD(angle);
	s = sin(rad);
	c = cos(rad);

	i = 1.0 - c;

	xx = (x * x) * i;
	yy = (y * y) * i;
	zz = (z * z) * i;
	xy = (x * y) * i;
	yz = (y * z) * i;
	zx = (z * x) * i;

	xs = x * s;
	ys = y * s;
	zs = z * s;

	mx[0] = m[ 0];
	mx[1] = m[ 1];
	mx[2] = m[ 2];
	mx[3] = m[ 3];
	my[0] = m[ 4];
	my[1] = m[ 5];
	my[2] = m[ 6];
	my[3] = m[ 7];
	mz[0] = m[ 8];
	mz[1] = m[ 9];
	mz[2] = m[10];
	mz[3] = m[11];

	rx[0] = xx + c;
	rx[1] = xy + zs;
	rx[2] = zx - ys;
	ry[0] = xy - zs;
	ry[1] = yy + c;
	ry[2] = yz + xs;
	rz[0] = zx + ys;
	rz[1] = yz - xs;
	rz[2] = zz + c;

	m[ 0] = mx[0] * rx[0] + my[0] * rx[1] + mz[0] * rx[2];
	m[ 1] = mx[1] * rx[0] + my[1] * rx[1] + mz[1] * rx[2];
	m[ 2] = mx[2] * rx[0] + my[2] * rx[1] + mz[2] * rx[2];
	m[ 3] = mx[3] * rx[0] + my[3] * rx[1] + mz[3] * rx[2];
	m[ 4] = mx[0] * ry[0] + my[0] * ry[1] + mz[0] * ry[2];
	m[ 5] = mx[1] * ry[0] + my[1] * ry[1] + mz[1] * ry[2];
	m[ 6] = mx[2] * ry[0] + my[2] * ry[1] + mz[2] * ry[2];
	m[ 7] = mx[3] * ry[0] + my[3] * ry[1] + mz[3] * ry[2];
	m[ 8] = mx[0] * rz[0] + my[0] * rz[1] + mz[0] * rz[2];
	m[ 9] = mx[1] * rz[0] + my[1] * rz[1] + mz[1] * rz[2];
	m[10] = mx[2] * rz[0] + my[2] * rz[1] + mz[2] * rz[2];
	m[11] = mx[3] * rz[0] + my[3] * rz[1] + mz[3] * rz[2];
}

void Matrix4_Scale (mat4_t m, float x, float y, float z){

	m[ 0] *= x;
	m[ 1] *= x;
	m[ 2] *= x;
	m[ 3] *= x;
	m[ 4] *= y;
	m[ 5] *= y;
	m[ 6] *= y;
	m[ 7] *= y;
	m[ 8] *= z;
	m[ 9] *= z;
	m[10] *= z;
	m[11] *= z;
}

void Matrix4_Translate (mat4_t m, float x, float y, float z){

	m[12] += m[ 0] * x + m[ 4] * y + m[ 8] * z;
	m[13] += m[ 1] * x + m[ 5] * y + m[ 9] * z;
	m[14] += m[ 2] * x + m[ 6] * y + m[10] * z;
	m[15] += m[ 3] * x + m[ 7] * y + m[11] * z;
}

void Matrix4_Shear (mat4_t m, float x, float y, float z){

	vec4_t	mx, my, mz;

	mx[0] = m[ 0] * x;
	mx[1] = m[ 1] * x;
	mx[2] = m[ 2] * x;
	mx[3] = m[ 3] * x;
	my[0] = m[ 4] * y;
	my[1] = m[ 5] * y;
	my[2] = m[ 6] * y;
	my[3] = m[ 7] * y;
	mz[0] = m[ 8] * z;
	mz[1] = m[ 9] * z;
	mz[2] = m[10] * z;
	mz[3] = m[11] * z;

	m[ 0] += my[0] + mz[0];
	m[ 1] += my[1] + mz[1];
	m[ 2] += my[2] + mz[2];
	m[ 3] += my[3] + mz[3];
	m[ 4] += mx[0] + mz[0];
	m[ 5] += mx[1] + mz[1];
	m[ 6] += mx[2] + mz[2];
	m[ 7] += mx[3] + mz[3];
	m[ 8] += mx[0] + my[0];
	m[ 9] += mx[1] + my[1];
	m[10] += mx[2] + my[2];
	m[11] += mx[3] + my[3];
}

void Matrix4_Transform (const mat4_t m, const vec4_t in, vec4_t out){

	out[0] = in[0] * m[ 0] + in[1] * m[ 4] + in[2] * m[ 8] + in[3] * m[12];
	out[1] = in[0] * m[ 1] + in[1] * m[ 5] + in[2] * m[ 9] + in[3] * m[13];
	out[2] = in[0] * m[ 2] + in[1] * m[ 6] + in[2] * m[10] + in[3] * m[14];
	out[3] = in[0] * m[ 3] + in[1] * m[ 7] + in[2] * m[11] + in[3] * m[15];

}
void Matrix4_Set (mat4_t m, const vec3_t rotation[3], const vec3_t translation){

	m[ 0] = rotation[0][0];
	m[ 1] = rotation[0][1];
	m[ 2] = rotation[0][2];
	m[ 3] = 0.0;
	m[ 4] = rotation[1][0];
	m[ 5] = rotation[1][1];
	m[ 6] = rotation[1][2];
	m[ 7] = 0.0;
	m[ 8] = rotation[2][0];
	m[ 9] = rotation[2][1];
	m[10] = rotation[2][2];
	m[11] = 0.0;
	m[12] = translation[0];
	m[13] = translation[1];
	m[14] = translation[2];
	m[15] = 1.0;
}

void Matrix4_Transpose (const mat4_t in, mat4_t out){

	out[ 0] = in[ 0];
	out[ 1] = in[ 4];
	out[ 2] = in[ 8];
	out[ 3] = in[12];
	out[ 4] = in[ 1];
	out[ 5] = in[ 5];
	out[ 6] = in[ 9];
	out[ 7] = in[13];
	out[ 8] = in[ 2];
	out[ 9] = in[ 6];
	out[10] = in[10];
	out[11] = in[14];
	out[12] = in[ 3];
	out[13] = in[ 7];
	out[14] = in[11];
	out[15] = in[15];
}

void Matrix4_Multiply (const mat4_t m1, const mat4_t m2, mat4_t out){

	out[ 0] = m1[ 0] * m2[ 0] + m1[ 4] * m2[ 1] + m1[ 8] * m2[ 2] + m1[12] * m2[ 3];
	out[ 1] = m1[ 1] * m2[ 0] + m1[ 5] * m2[ 1] + m1[ 9] * m2[ 2] + m1[13] * m2[ 3];
	out[ 2] = m1[ 2] * m2[ 0] + m1[ 6] * m2[ 1] + m1[10] * m2[ 2] + m1[14] * m2[ 3];
	out[ 3] = m1[ 3] * m2[ 0] + m1[ 7] * m2[ 1] + m1[11] * m2[ 2] + m1[15] * m2[ 3];
	out[ 4] = m1[ 0] * m2[ 4] + m1[ 4] * m2[ 5] + m1[ 8] * m2[ 6] + m1[12] * m2[ 7];
	out[ 5] = m1[ 1] * m2[ 4] + m1[ 5] * m2[ 5] + m1[ 9] * m2[ 6] + m1[13] * m2[ 7];
	out[ 6] = m1[ 2] * m2[ 4] + m1[ 6] * m2[ 5] + m1[10] * m2[ 6] + m1[14] * m2[ 7];
	out[ 7] = m1[ 3] * m2[ 4] + m1[ 7] * m2[ 5] + m1[11] * m2[ 6] + m1[15] * m2[ 7];
	out[ 8] = m1[ 0] * m2[ 8] + m1[ 4] * m2[ 9] + m1[ 8] * m2[10] + m1[12] * m2[11];
	out[ 9] = m1[ 1] * m2[ 8] + m1[ 5] * m2[ 9] + m1[ 9] * m2[10] + m1[13] * m2[11];
	out[10] = m1[ 2] * m2[ 8] + m1[ 6] * m2[ 9] + m1[10] * m2[10] + m1[14] * m2[11];
	out[11] = m1[ 3] * m2[ 8] + m1[ 7] * m2[ 9] + m1[11] * m2[10] + m1[15] * m2[11];
	out[12] = m1[ 0] * m2[12] + m1[ 4] * m2[13] + m1[ 8] * m2[14] + m1[12] * m2[15];
	out[13] = m1[ 1] * m2[12] + m1[ 5] * m2[13] + m1[ 9] * m2[14] + m1[13] * m2[15];
	out[14] = m1[ 2] * m2[12] + m1[ 6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
	out[15] = m1[ 3] * m2[12] + m1[ 7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
}

void MatrixMultiply (const vec3_t in1[3], const vec3_t in2[3], vec3_t out[3]){

	out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
	out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
	out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
	out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
	out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
	out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
	out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
	out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
	out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
}

void AxisTranspose (const vec3_t in[3], vec3_t out[3]){

	out[0][0] = in[0][0];
	out[0][1] = in[1][0];
	out[0][2] = in[2][0];
	out[1][0] = in[0][1];
	out[1][1] = in[1][1];
	out[1][2] = in[2][1];
	out[2][0] = in[0][2];
	out[2][1] = in[1][2];
	out[2][2] = in[2][2];
}

void Matrix4_TransformVector (const mat4_t m, const vec3_t in, vec3_t out){

	out[0] = in[0] * m[ 0] + in[1] * m[ 4] + in[2] * m[ 8] + m[12];
	out[1] = in[0] * m[ 1] + in[1] * m[ 5] + in[2] * m[ 9] + m[13];
	out[2] = in[0] * m[ 2] + in[1] * m[ 6] + in[2] * m[10] + m[14];
}

void Matrix4_TransformNormal (const mat4_t m, const vec3_t in, vec3_t out){

	out[0] = in[0] * m[ 0] + in[1] * m[ 4] + in[2] * m[ 8];
	out[1] = in[0] * m[ 1] + in[1] * m[ 5] + in[2] * m[ 9];
	out[2] = in[0] * m[ 2] + in[1] * m[ 6] + in[2] * m[10];
}